package org.movsim.simulator.vehicles;

import org.movsim.simulator.B5Constants.INPUT_FROM_ENVIRONMENT;
import org.movsim.simulator.MovsimConstants;
import org.movsim.simulator.roadnetwork.LaneSegment;
import org.movsim.simulator.roadnetwork.RoadSegment;
import org.movsim.simulator.vehicles.longitudinalmodel.TrafficLightApproaching;

public class Environment {

    private PSEnabledVehicle v;
    private DegreeOfEnvironment[] env = new DegreeOfEnvironment[INPUT_FROM_ENVIRONMENT.values().length];

    public Environment(PSEnabledVehicle v) {
        // TODO Auto-generated constructor stub
        this.v = v;
        env[INPUT_FROM_ENVIRONMENT.DEGREE_OF_CONGESTION.ordinal()] = new DegreeOfCongestion();
        env[INPUT_FROM_ENVIRONMENT.DEGREE_OF_JAM.ordinal()] = new DegreeOfJam();
        env[INPUT_FROM_ENVIRONMENT.DEGREE_OF_DOWN_STREAM_SIGNAL_DISTANCE.ordinal()] = new DegreeOfDownStreamSignalDistance();
        env[INPUT_FROM_ENVIRONMENT.DEGREE_OF_OVER_SPEED_AND_UNDER_SPEED.ordinal()] = new DegreeOfOverSpeedAndUnderSpeed();
        env[INPUT_FROM_ENVIRONMENT.DEGREE_OF_DISTANCE_FROM_TRAILING_VEHICLE.ordinal()] = new DegreeOfDistanceFromTrailingVehicle();
        env[INPUT_FROM_ENVIRONMENT.DEGREE_OF_DISTANCE_FROM_FRONT_VEHICLE.ordinal()] = new DegreeOfDistanceFromFrontVehicle();
        env[INPUT_FROM_ENVIRONMENT.DEGREE_OF_INSTABILITY_OF_LEFT_LANE.ordinal()] = new DegreeOfInStabilityOfLeftLane();
        env[INPUT_FROM_ENVIRONMENT.DEGREE_OF_INSTABILITY_OF_RIGHT_LANE.ordinal()] = new DegreeOfInStabilityOfRightLane();

    }

    public double getDegreeOfEnvironment(INPUT_FROM_ENVIRONMENT e, RoadSegment roadSegment) {
        return env[e.ordinal()].get(roadSegment);
    }

    private class NoLaneExistException extends Exception {

        public NoLaneExistException() {
            super();
            // TODO Auto-generated constructor stub
        }

        /**
         * 
         */
        private static final long serialVersionUID = 1L;
    }

    private double getDegreeOfCongestion(RoadSegment roadSegment, int laneId) throws NoLaneExistException {
        double degreeOfCongestion = 0;
        LaneSegment laneSegment = roadSegment.laneSegment(laneId);
        if (laneSegment != null && laneSegment.vehicleCount() > 0) {
            double totalVehicleLength = 0;
            for (final Vehicle v : laneSegment) {
                totalVehicleLength += (v.getLength() + .5);
            }
            double averageVehicleLength = totalVehicleLength / laneSegment.vehicleCount();

            double laneCapacity = (laneSegment.roadLength() / averageVehicleLength);
            degreeOfCongestion = laneSegment.vehicleCount() / laneCapacity;
        } else {
            throw new NoLaneExistException();
        }
        return degreeOfCongestion;
    }

    /*
     * The following method computes average congestion
     * TODO: Must give maximum weigh to congestion of nearest lane and less weigh to farthest lanes instead of giving equal weigh to
     * each lane
     */
    // E1
    public double getDegreeOfCongestion(RoadSegment roadSegment) {
        int laneCount = roadSegment.laneCount();
        double totalCongestion = 0;
        double avgCongestion = 0;
        try {
            totalCongestion += getDegreeOfCongestion(roadSegment, v.lane());
        } catch (NoLaneExistException e) {
        }
        for (int i = v.lane() + 1; i <= laneCount; ++i) {
            try {
                totalCongestion += getDegreeOfCongestion(roadSegment, i);
            } catch (NoLaneExistException e) {
                break;
            }
        }
        for (int i = v.lane() - 1; i > 0; --i) {
            try {
                totalCongestion += getDegreeOfCongestion(roadSegment, i);
            } catch (NoLaneExistException e) {
                break;
            }
        }
        avgCongestion = totalCongestion / laneCount;
        return avgCongestion;
    }

    private double getDegreeOfJam(RoadSegment roadSegment, int laneId) throws NoLaneExistException {
        double degreeOfJam = 0;
        LaneSegment laneSegment = roadSegment.laneSegment(laneId);
        if (laneSegment != null && laneSegment.vehicleCount() > 0) {
            degreeOfJam = (double) laneSegment.stoppedVehicleCount() / laneSegment.vehicleCount();
        } else {
            throw new NoLaneExistException();
        }
        return degreeOfJam;
    }

    /*
     * The following method computes average jam
     * TODO: Must give maximum weigh to jam of nearest lane and less weigh to farthest lanes instead of giving equal weigh to each
     * lane
     */
    // E2
    public double getDegreeOfJam(RoadSegment roadSegment) {
        int laneCount = roadSegment.laneCount();
        double totalJam = 0;
        double avgJam = 0;
        try {
            totalJam += getDegreeOfJam(roadSegment, v.lane());
        } catch (NoLaneExistException e) {
        }
        for (int i = v.lane() + 1; i <= laneCount; ++i) {
            try {
                totalJam += getDegreeOfJam(roadSegment, i);
            } catch (NoLaneExistException e) {
                break;
            }
        }
        for (int i = v.lane() - 1; i > 0; --i) {
            try {
                totalJam += getDegreeOfJam(roadSegment, i);
            } catch (NoLaneExistException e) {
                break;
            }
        }
        avgJam = totalJam / laneCount;
        return avgJam;
    }

    // E3
    public double getDegreeOfDownStreamSignalDistance(RoadSegment roadSegment) {
        // TODO Auto-generated method stub
        double retval = v.getDistanceFromSignal();
        retval = sigmoid(retval, 0, TrafficLightApproaching.getMaxLookAheadDistance(v));
        return retval;
    }

    public double getDegreeOfOverSpeed() {
        double degreeOfOverSpeed = (v.getSpeed() - v.getSpeedlimit()) / MovsimConstants.MAX_VEHICLE_SPEED;
        return degreeOfOverSpeed > 0 ? degreeOfOverSpeed : 0;
    }

    public double getDegreeOfUnderSpeed() {
        double degreeOfUnderSpeed = (v.getSpeedlimit() - v.getSpeed()) / MovsimConstants.MAX_VEHICLE_SPEED;
        return degreeOfUnderSpeed > 0 ? degreeOfUnderSpeed : 0;
    }

    // E4
    public double getDegreeOfOverSpeedAndUnderSpeed(RoadSegment roadSegment) {
        double degree = getDegreeOfOverSpeed();
        return degree > 0 ? degree : getDegreeOfUnderSpeed();
    }


    // E6
    public double getDegreeOfDistanceFromTrailingVehicle(RoadSegment roadSegment) {
        LaneSegment laneSegment = roadSegment.laneSegment(v.lane());
        Vehicle rearVehicle = laneSegment.rearVehicle(v.getRearPosition() - 1);
        double distanceFromRearVehicle = v.getNetDistanceToRearVehicle(rearVehicle);
        distanceFromRearVehicle = distanceFromRearVehicle > 0 ? distanceFromRearVehicle : 0;
        return sigmoid(distanceFromRearVehicle, 0, TrafficLightApproaching.getMaxLookAheadDistance(v));
    }

    // E7
    public double getDegreeOfDistanceFromFrontVehicle(RoadSegment roadSegment) {
        LaneSegment laneSegment = roadSegment.laneSegment(v.lane());
        Vehicle frontVehicle = laneSegment.frontVehicle(v);
        double distanceFromFrontVehicle = v.getNetDistance(frontVehicle);
        return sigmoid(distanceFromFrontVehicle, 0, TrafficLightApproaching.getMaxLookAheadDistance(v));
    }

    // E8
    public double getDegreeOfInStabilityOfLeftLane(RoadSegment roadSegment) {
        double retval = 0;
        double speed = 0;
        double leftAvgSpeed = 0;
        double speedDiff = 0;
        speed = v.getSpeed();
        int leftLane = v.lane() - 1;
        if (leftLane > 0) {
            LaneSegment leftLaneSegment = roadSegment.laneSegment(leftLane);
            if (leftLaneSegment.vehicleCount() - leftLaneSegment.stoppedVehicleCount() > 0) {
                leftAvgSpeed = leftLaneSegment.getAverageSpeed();
                speedDiff = leftAvgSpeed - speed;
                if (speedDiff < 0) {
                    speedDiff = -speedDiff;
                    retval = speedDiff / speed;
                } else if (speedDiff < v.STABLE_SPEED_DIFFERENCE) {
                    retval = 0;
                } else {
                    retval = speedDiff / (MovsimConstants.MAX_VEHICLE_SPEED - speed - v.STABLE_SPEED_DIFFERENCE);
                }
            }
        }
        return retval;
    }

    // E9
    public double getDegreeOfInStabilityOfRightLane(RoadSegment roadSegment) {
        double retval = 0;
        double speed = 0;
        double rightAvgSpeed = 0;
        double speedDiff = 0;
        speed = v.getSpeed();
        int rightLane = v.lane() + 1;
        if (rightLane <= roadSegment.laneCount()) {
            LaneSegment rightLaneSegment = roadSegment.laneSegment(rightLane);
            if (rightLaneSegment.vehicleCount() - rightLaneSegment.stoppedVehicleCount() > 0) {
                rightAvgSpeed = rightLaneSegment.getAverageSpeed();
                speedDiff = speed - rightAvgSpeed;
                if (speedDiff < 0) {
                    speedDiff = -speedDiff;
                    retval = speedDiff / (MovsimConstants.MAX_VEHICLE_SPEED - speed);
                } else if (speedDiff < v.STABLE_SPEED_DIFFERENCE) {
                    retval = 0;
                } else {
                    retval = (speed - v.STABLE_SPEED_DIFFERENCE - rightAvgSpeed) / (speed - v.STABLE_SPEED_DIFFERENCE);
                }
            }
        }
        return retval;
    }

    public static double sigmoid(double x, double min, double max) {
        if (max < min) {
            throw new IllegalArgumentException();
        }
        double range = max - min;
        double meu = range / 2;
        double s = range / 8;
        return 1 / (1 + Math.exp(-(x - meu) / s));
    }

    public abstract class DegreeOfEnvironment {
        public abstract double get(RoadSegment roadSegment);
    }

    public class DegreeOfCongestion extends DegreeOfEnvironment {

        @Override
        public double get(RoadSegment roadSegment) {
            // TODO Auto-generated method stub
            return getDegreeOfCongestion(roadSegment);
        }

    }

    public class DegreeOfJam extends DegreeOfEnvironment {

        @Override
        public double get(RoadSegment roadSegment) {
            // TODO Auto-generated method stub
            return getDegreeOfJam(roadSegment);
        }

    }

    public class DegreeOfDownStreamSignalDistance extends DegreeOfEnvironment {

        @Override
        public double get(RoadSegment roadSegment) {
            // TODO Auto-generated method stub
            return getDegreeOfDownStreamSignalDistance(roadSegment);
        }

    }

    public class DegreeOfOverSpeedAndUnderSpeed extends DegreeOfEnvironment {

        @Override
        public double get(RoadSegment roadSegment) {
            // TODO Auto-generated method stub
            return getDegreeOfOverSpeedAndUnderSpeed(roadSegment);
        }

    }

    public class DegreeOfDistanceFromTrailingVehicle extends DegreeOfEnvironment {

        @Override
        public double get(RoadSegment roadSegment) {
            // TODO Auto-generated method stub
            return getDegreeOfDistanceFromTrailingVehicle(roadSegment);
        }

    }

    public class DegreeOfDistanceFromFrontVehicle extends DegreeOfEnvironment {

        @Override
        public double get(RoadSegment roadSegment) {
            // TODO Auto-generated method stub
            return getDegreeOfDistanceFromFrontVehicle(roadSegment);
        }

    }

    public class DegreeOfInStabilityOfLeftLane extends DegreeOfEnvironment {

        @Override
        public double get(RoadSegment roadSegment) {
            // TODO Auto-generated method stub
            return getDegreeOfInStabilityOfLeftLane(roadSegment);
        }

    }

    public class DegreeOfInStabilityOfRightLane extends DegreeOfEnvironment {

        @Override
        public double get(RoadSegment roadSegment) {
            // TODO Auto-generated method stub
            return getDegreeOfInStabilityOfRightLane(roadSegment);
        }

    }

}
